#ifndef CMD_FSM_H
#define CMD_FSM_H

#include<ros/ros.h>
#include<Eigen/Eigen>
#include<mavros_msgs/ManualControl.h>
#include<mavros_msgs/State.h>
#include<mavros_msgs/PositionTarget.h>
#include<quadrotor_msgs/PositionCommand.h>
#include<nav_msgs/Odometry.h>
#include<std_msgs/Int16.h>
#include<std_msgs/Bool.h>
#include<tf2/LinearMath/Quaternion.h>
#include<tf2/LinearMath/Matrix3x3.h>
#include<tf2_geometry_msgs/tf2_geometry_msgs.h>
// #include"obstacle_avoid/obstacle_avoid_fsm.h"

namespace obstacle_avoid
{
    class CONTROL_CMD_FSM
    {
    private:
        //paramters
        //0 for INIT, 1 for MANUAL_FLIGHT, 2 for WARNING_ALERT, 3 for OPERATOR_LIMIT, 4 for ERMERGENCY_HOVER, 5 for RETURN_HOME
        int cmd_state;
        bool have_manual_cmd;
        bool have_odom;
        bool have_traj_cmd;
        double max_horizonal_vel;
        double max_vertical_vel;
        double max_yaw_rate;
        double vel_limit_factor;
        double emergency_hover_duration;

        //data
        geometry_msgs::PoseStamped home_pos;
        std_msgs::Bool start_return;
        nav_msgs::Odometry odom;
        mavros_msgs::ManualControl user_cmd;
        mavros_msgs::State uav_state;
        mavros_msgs::PositionTarget current_cmd;
        quadrotor_msgs::PositionCommand traj_cmd;
        Eigen::Quaterniond odom_orient;
        Eigen::Vector3d odom_vel;
        Eigen::Vector3d odom_pos;
        Eigen::Vector3d last_motion_direction;
        bool is_backing_up;
        bool is_armed_prev;

        //ROS utils
        ros::NodeHandle node;
        ros::Timer control_cmd_timer;
        ros::Publisher cmd_pub;
        ros::Publisher home_pos_pub;
        ros::Subscriber odom_sub;
        ros::Subscriber manual_cmd_sub;
        ros::Subscriber state_sub;
        ros::Subscriber obs_state_sub;
        ros::Subscriber start_return_sub;
        ros::Subscriber traj_cmd_sub;

        //ROS Function
        void cmdTimerCallback(const ros::TimerEvent &e);
        void manualCmdCallback(const mavros_msgs::ManualControlConstPtr &msg);
        void stateCallback(const mavros_msgs::StateConstPtr &msg);
        void odomCallback(const nav_msgs::OdometryConstPtr &msg);
        void obsStateCallback(const std_msgs::Int16ConstPtr &msg);
        void returnHomeCallback(const std_msgs::BoolConstPtr &msg);
        void trajCmdCallback(const quadrotor_msgs::PositionCommandConstPtr &msg);

        //helper funcion
        //record home position
        void recordHomePosition();
        void resetCurrentCmd(int cmd_state);
        float handleZCmd(float z_input);
        float handleYawCmd(float r_input);
        void printfCmdState();

    public:
        void init(ros::NodeHandle &nh);   
        
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    };

}// namespace obstacle_avoid

#endif  